// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "serial/serial.h"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include <iostream>
#include <chrono>
#include <thread>

serial::Serial ros_ser;

// 开启 01 05 00 00 FF 00
uint8_t opendata[8] = {0x01, 0x05, 0x00, 0x00, 0xff, 0x00, 0x8c, 0x3a};
// 关闭
uint8_t closedata[8] = {0x01, 0x05, 0x00, 0x00, 0x00, 0x00, 0xcd, 0xca};

// 开启 02 05 00 00 FF 00
uint8_t opendata2[8] = {0x02, 0x05, 0x00, 0x00, 0xff, 0x00, 0x8c, 0x09};
// 关闭 02 05 00 00 00 00
uint8_t closedata2[8] = {0x02, 0x05, 0x00, 0x00, 0x00, 0x00, 0xcd, 0xf9};

// 台号1
uint8_t nazzle_valves1[9] = {0X01, 0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
uint8_t OPEN_1_1[9] = {0X01, 0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
uint8_t OPEN_1_2[9] = {0X01, 0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
uint8_t OPEN_1_3[9] = {0X01, 0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
uint8_t OPEN_1_4[9] = {0X01, 0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
uint8_t CLOSE_1[9] = {0X01, 0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
// 台号2
uint8_t nazzle_valves2[9] = {0X02, 0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
uint8_t OPEN_2_1[9] = {0X02, 0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
uint8_t OPEN_2_2[9] = {0X02, 0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
uint8_t OPEN_2_3[9] = {0X02, 0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
uint8_t OPEN_2_4[9] = {0X02, 0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
uint8_t CLOSE_2[9] = {0X02, 0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};

// 3.自定义节点类；
class Big_Valves : public rclcpp::Node
{
public:
    Big_Valves() : Node("big_valves_node_cpp")
    {
        this->state = this->create_subscription<std_msgs::msg::Int32MultiArray>("big_valve_state", 10,
                                                                                std::bind(&Big_Valves::valveCallback,
                                                                                          this, std::placeholders::_1));
        ros_ser.write(opendata, 8);
    }

private:
    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr state;
    int valve_state_1 = 0; // 用于存储阀门状态的变量
    int valve_state_2 = 0; // 用于存储阀门状态的变量

    // can阀门量
    int nazzle_valves1_can = 0;
    int nazzle_valves2_can = 0;

    void valveCallback(const std_msgs::msg::Int32MultiArray::SharedPtr state_msg)
    {
        valve_state_1 = state_msg->data[0];
        valve_state_2 = state_msg->data[1];
        nazzle_valves1_can = state_msg->data[2];
        nazzle_valves2_can = state_msg->data[3];
        switch (valve_state_1)
        {
        case 0:
            ros_ser.write(closedata, 8);
            std::cout << "阀门1关闭" << std::endl;
            break;

        case 1:
            ros_ser.write(opendata, 8);
            std::cout << "阀门1开启" << std::endl;

            break;
        default:
            break;
        }
        ros_ser.flush();
        // pause for 500 ms
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        switch (valve_state_2)
        {
        case 0:
            ros_ser.write(closedata2, 8);
            std::cout << "阀门2关闭" << std::endl;

            break;

        case 1:
            ros_ser.write(opendata2, 8);
            std::cout << "阀门2开启" << std::endl;

            break;

        default:
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        ros_ser.flush();
        switch (nazzle_valves1_can)
        {
        case 0:
            ros_ser.write(CLOSE_1, 9);
            std::cout << "喷嘴阀门1关闭" << std::endl;
            break;
        case 1:
            ros_ser.write(OPEN_1_1, 9);
            std::cout << "喷嘴阀门1开启1" << std::endl;
            break;
        case 2:
            ros_ser.write(OPEN_1_2, 9);
            std::cout << "喷嘴阀门1开启2" << std::endl;
            break;
        case 3:
            ros_ser.write(OPEN_1_3, 9);
            std::cout << "喷嘴阀门1开启3" << std::endl;
            break;
        case 4:
            ros_ser.write(OPEN_1_4, 9);
            std::cout << "喷嘴阀门1开启4" << std::endl;
            break;
        default:
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        ros_ser.flush();
        switch (nazzle_valves2_can)
        {
        case 0:
            ros_ser.write(CLOSE_2, 9);
            std::cout << "喷嘴阀门2关闭" << std::endl;
            break;
        case 1:
            ros_ser.write(OPEN_2_1, 9);
            std::cout << "喷嘴阀门2开启1" << std::endl;
            break;
        case 2:
            ros_ser.write(OPEN_2_2, 9);
            std::cout << "喷嘴阀门2开启2" << std::endl;
            break;
        case 3:
            ros_ser.write(OPEN_2_3, 9);
            std::cout << "喷嘴阀门2开启3" << std::endl;
            break;
        case 4:
            ros_ser.write(OPEN_2_4, 9);
            std::cout << "喷嘴阀门2开启4" << std::endl;
            break;
        default:
            break;
        }
        ros_ser.flush();
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        std::cout << "" << std::endl;
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 连接串口
    ros_ser.setPort("/dev/ttyS1");
    // 串口波特率
    ros_ser.setBaudrate(9600);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ros_ser.setTimeout(to);
    try
    {
        ros_ser.open();
    }
    catch (serial::IOException &e)
    {
        std::cout << "unable to open" << std::endl;
        return -1;
    }
    if (ros_ser.isOpen())
    {
        std::cout << "-----open------" << std::endl;
    }
    else
    {
        return -1;
    }
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Big_Valves>());
    // 5.资源释放
    rclcpp::shutdown();
    // 关闭串口
    ros_ser.write(closedata, 8);
    // ros_ser.write(closedata2, 8);
    ros_ser.close();
    return 0;
}